/*
 *  Extended Kalman Filter estimator
 *  Copyright (C) 2012, CYPHY lab
 *  Inkyu Sa <i.sa@qut.edu.au>
 *
 *  http://wiki.qut.edu.au/display/cyphy
 *
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */


#include "cyphy_ekf_pole_cam_imu.h"
/*
using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::math;
*/
using namespace std;
using namespace Eigen;

int main(int argc, char** argv)
{
	ROS_INFO("Cyphy ROS EKF pole node");
	ros::init(argc, argv, "cyphy_ekf_pole");
	ros::NodeHandle nh;
	ekf_pole ekf_pole(nh);  

/*
	//CTicTac tictac;
	double T;  

	
	measured_line<< 0.0329,113.2937,0.0470,154.2739;

	
	measured_ang<< -0.0140,-0.0122;

	

	cyphy_ekf_pole.ekf_init();

	ros::Time begin = ros::Time::now();
	//tictac.Tic();
	cyphy_ekf_pole.cam_imu_EKF(&measured_line,&measured_ang,&out);
	ros::Time end = ros::Time::now();
	ROS_INFO_STREAM("Time: " << (end-begin)*1e6<<"us"<<endl);
  	
	//T=tictac.Tac();
	//cout << "Time: " << T*1e6 << " us" << endl;
	cout<<"y = "<<out.y<<endl;
	cout<<"cov = "<<out.covs<<endl;
*/

	ros::spin();

/*

	while(ros::ok()) 
	{
		ros::spinOnce();
		//tictac.Tic();
		ekf_test.cam_imu_EKF(&measured_line,&measured_ang,&out);
		//T=tictac.Tac();
		//cout << "Time: " << T*1e6 << " us" << endl;
		//cout<<"y = "<<out.y<<endl;
		//cout<<"cov = "<<out.covs<<endl;
	}
  */




  /*
  CTicTac tictac;
  double T;  
  double jac_out[2][9];
  double (*jac_pt)[9];
  jac_pt = jac_out;
  double line_out[2][1];
  double (*line_pt)[1];
  line_pt=line_out;
  double Ax,Ay,Az,Bx,By,Bz,c_x,c_y,c_z,c_dx,c_dy,c_dz,phi,theta,psi,fx,fy,px,py;

  px=200;
  py=200;
  fx=267;
  fy=267;
  Ax=0;
  Ay=0;
  Az=0;
  Bx=0;
  By=0;
  Bz=0.3;
  c_x=0.1;
  c_y=0.2;
  c_z=0.3;
  c_dx=0.01;
  c_dy=0.02;
  c_dz=0.03;
  phi=0.4;
  theta=0.5;
  psi=0.6;

  tictac.Tic();
 
  

  //Measure time here
  line_model(Ax,Ay,Az ,Bx,By,Bz ,c_x,c_y,c_z,fx,fy,px,py,  phi,theta,psi, line_pt);
  jac(Ax,Ay,Az ,Bx,By,Bz ,c_x,c_y,c_z,fx,fy,px,py,  phi,theta,psi, jac_pt);

  T=tictac.Tac();
  cout << "Time: " << T*1e6 << " us" << endl;
  cout <<"Jac"<<endl;
  for(int i=0;i<2;i++) 
  {
	for(int j=0;j<9;j++) printf("%.4f,",jac_out[i][j]);
	printf("\n");
  }

  cout <<"Line"<<endl;


  for(int i=0;i<2;i++) 
  {
	for(int j=0;j<1;j++) printf("%.4f,",line_out[i][j]);
	printf("\n");
  }

  */
  return 0;
}
